#include "HALL_Function.h"
#include "Parameter.h"
#include "SVPWM.h"
#include "FOC.h"
#include "DAC.h"

void Theta_Calculate(void)
{
	  Sin_Normalization=((Sin_value-sin_dc_component)*1000)/(sin_max-sin_dc_component);
	  Cos_Normalization=((Cos_value-cos_dc_component)*1000)/(cos_max-cos_dc_component);
		Angle_PID();
}

void Speed_calculate(void)
{
		angle_value=Angle_feedback;
		if(angle_value>=angle_value_last)
		{
				angle_gap[angle_count]=angle_value-angle_value_last;
				Angle_value+=angle_gap[angle_count];
				angle_count++;
		}
		if(angle_count==10)
		{	
				Speed=Angle_value*5/3;
				Angle_value=0;
				angle_gap[0]=0;
				angle_gap[1]=0;
				angle_gap[2]=0;
				angle_gap[3]=0;
				angle_gap[4]=0;
				angle_gap[5]=0;
				angle_gap[6]=0;
				angle_gap[7]=0;
				angle_gap[8]=0;
				angle_gap[9]=0;
				angle_count=0;
		}
		angle_value_last=angle_value;
}

void Speed_Calculate(void)
{
	if(speed_count>7)
		{
			speed_count=0;
			speed_get_flag=1;
		}
		speed_time[speed_count++]=((CCU40_CC42->CV[0]&0x0000FFFF)<<16)+(CCU40_CC41->CV[0]&0x0000FFFF);	
		if(speed_get_flag==1)
		{
				speed_time_sum=((speed_time[0]+speed_time[1]+speed_time[2]+speed_time[3]+speed_time[4]+speed_time[5]+speed_time[6]+speed_time[7])>>3);
		}		
		Speed=112500000/speed_time_sum;	
}

